/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Watchdog;
import java.util.Date;
import java.util.Timer;
import java.util.TimerTask;

public class DefaultRobot extends IterativeRobot
{
    DriverStation m_ds;
    Joystick driverJoy;
    Joystick shooterJoy;
    Kicker kicker;
    Lifter lifter;
    DriveTrain dt;

    long millis;

    Timer dtTimer;
    Timer mmTimer;
    boolean mmTimerStarted = false;
    boolean mmEntered = false;
    private Compressor compressor;

    boolean shutdownInit = false;
    boolean shutdownFinished = false;

    boolean dtInitialized = false;

    TimerTask maintModeTT;

    boolean cleanupMM = false;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit()
    {
        m_ds = DriverStation.getInstance();

        shooterJoy = new Joystick(1);   //set joystick to port 1
        driverJoy = new Joystick(2);    //set joystick to port 2
        dt = new DriveTrain(driverJoy);

        compressor = new Compressor(6,5);
        compressor.start();

        kicker = new Kicker(shooterJoy);
        lifter = new Lifter(shooterJoy);
    }

    

    /********************************** Init Routines *************************************/
    public void disabledInit()
    {
    }

    public void autonomousInit()
    {

    }

    public void teleopInit()
    {
        shutdownInit = true; 
    }

    /********************************** Periodic Routines *************************************/
    public void disabledPeriodic()
    {
        // feed the user watchdog at every period when disabled
        Watchdog.getInstance().feed();

        if(shutdownInit && !shutdownFinished)
        {
            dt.shutDown();
            shutdownFinished = true;
            enableDTTask(false);
        }

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic()
    {
        Watchdog.getInstance().feed();

        initDT();

        /**
         * Read file and get which autonomous mode we are using.
         * IF 1
         *      Drive forward hardcoded amount and kick ball.
         *      Ball is expected to be directly in front of goal.
         *      (May need to rotate a little to align with goal?)
         * IF 2
         *      Wait 5 seconds
         *      Drive forward and kick first ball
         *      Backup, move right and slightly rotate to align with target
         *      Move forward and kick second ball
         *
         * IF 3
         *      Wait 7 seconds
         *      Drive forward and kick first ball
         *      Backup, move right and slightly rotate to align with target
         *      Kick second ball
         *      Backup, move right and adjust slightly to align with target
         *      Move forward and kick third ball
         */
    }

    /**
     * This function is called periodically during operator control. Check whether
     * the kicker is pressing a button on the joystick. All DT tasks are being
     * completed in a separate thread.
     */
    public void teleopPeriodic()
    {
        Watchdog.getInstance().feed();

        initDT();

        checkMMTimer();

        kicker.Run();
        lifter.Run();
        



//        if(driverJoy.getRawButton(Utils.TARGET_TUNE_BUTTON_NUM))
//        {
//            dt.getCurrPID().disable();
//            TargetTuner.tuneTarget(driverJoy);
//            dt.getCurrPID().setSetpoint(TargetTuner.TARGET);
//            dt.getCurrPID().enable();
//        }

//        if(driverJoy.getRawButton(Utils.PID_TUNE_BUTTON_NUM))
//        {
//            dt.getCurrPID().disable();
//            PIDDebugUtil.enterPIDDebug(dt.getCurrPID(), driverJoy, "None");
//            dt.getCurrPID().enable();
//
//        }
//        System.out.println("------------------------");
//        System.out.println("Raw value: " + dt.getCurrEnc().getValue());
//        System.out.println("Numturns: " + dt.getCurrEnc().numTurns);
//        System.out.println("Calc angle: " + dt.getCurrEnc().getCurrentAngle());
//        System.out.println("Error: " + dt.getCurrPID().getError());
//        System.out.println("Output: " + dt.getCurrPID().get());
    }

    private void enableDTTask(boolean enable)
    {
        Watchdog.getInstance().feed();
        if(enable)
        {
            dtTimer = Utils.getNewTimerObject();

            dtTimer.schedule(dt, new Date(), 10);

        }
        else
        {
            dtTimer.cancel();
        }

    }

    private void initDT()
    {
        if(!dtInitialized)
        {
            dt.initialize();

            enableDTTask(true);

            dtInitialized = true;
        }
    }

    private void checkMMTimer()
    {
        Watchdog.getInstance().feed();

        if(mmEntered && cleanupMM)
        {

            Watchdog.getInstance().feed();

            cancelMMTimer();

            mmEntered = false;

            cleanupMM = false;

            dt.pauseDT(false);

            return;
        }
        //Cancel timer if the button is still not pressed before event is fired.
        if(!dt.maintenanceModeButtonPressed() && mmTimerStarted && !mmEntered)
        {
            cancelMMTimer();
        }
        else if(dt.maintenanceModeButtonPressed() && !mmTimerStarted && !mmEntered)
        {
            mmTimer = Utils.getNewTimerObject();
            try
            {
                maintModeTT.cancel();

                setupMMTT();
            }
            catch(Exception ex)
            {

                setupMMTT();

            }

            mmTimer.schedule(maintModeTT, 2000);
            mmTimerStarted = true;

        }
    }

    private void setupMMTT()
    {
        maintModeTT = new TimerTask()
                    {
                        public void run()
                        {
                            dt.pauseDT(true);
                            mmEntered = true;

                            mmTimerStarted = false;

                            dt.reenterMaintenanceMode();

                            Watchdog.getInstance().feed();

                            cleanupMM = true;

                        }
                    };
    }

    private void cancelMMTimer()
    {
            mmTimer.cancel();
    }
}
